#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <Eigen/Eigen>
#include <iostream>
#include <string>

geometry_msgs::PoseStamped uav_t265;
geometry_msgs::PoseStamped uav_vicon;
geometry_msgs::PoseStamped obs_vicon;

bool have_uav_t265 = false;
bool have_uav_vicon = false;
bool have_obs_vicon = false;

void T265UAVPosCb(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
  uav_t265 = *msg;
  have_uav_t265 = true;
}

void ViconUAVPosCb(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
  uav_vicon = *msg;
  have_uav_vicon = true;
}

void ViconOBSPosCb(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
  obs_vicon = *msg;
  have_obs_vicon = true;
}

int main(int argc, char *argv[])
{
  ros::init(argc, argv, "trans_obs_from_vicon2t265");
  ros::NodeHandle nh;

  ros::Publisher pubPose = nh.advertise<geometry_msgs::PoseStamped>(
      "/t265/obs/pos", 10);
  ros::Subscriber subT265UAVPos = nh.subscribe("/t265/uav/pos", 10, T265UAVPosCb);
  ros::Subscriber subViconUAVPos = nh.subscribe("/vicon/uav/pos", 10, ViconUAVPosCb);
  ros::Subscriber subViconOBSPos = nh.subscribe("/vicon/obs/pos", 10, ViconOBSPosCb);
  ros::Rate rate(50);

  while (ros::ok())
  {
    ros::spinOnce();

    if (!have_uav_t265 || !have_uav_vicon || !have_obs_vicon)
      continue;
    
    have_uav_t265 = false;
    have_uav_vicon = false;
    have_obs_vicon = false;

    geometry_msgs::PoseStamped obs_t265;

    obs_t265.header = uav_t265.header;

    Eigen::Quaterniond q_t2652uav(uav_t265.pose.orientation.w,
                                  uav_t265.pose.orientation.x,
                                  uav_t265.pose.orientation.y,
                                  uav_t265.pose.orientation.z);
    Eigen::Quaterniond q_vicon2uav(uav_vicon.pose.orientation.w,
                                  uav_vicon.pose.orientation.x,
                                  uav_vicon.pose.orientation.y,
                                  uav_vicon.pose.orientation.z);
    Eigen::Quaterniond q_2652vicon = q_t2652uav * q_vicon2uav.inverse();

    Eigen::Vector3d p_obs_vicon(obs_vicon.pose.position.x,
                                obs_vicon.pose.position.y,
                                obs_vicon.pose.position.z);
    Eigen::Vector3d p_uav_vicon(uav_vicon.pose.position.x,
                                uav_vicon.pose.position.y,
                                uav_vicon.pose.position.z);
    Eigen::Vector3d p_uav_t265(uav_t265.pose.position.x,
                                uav_t265.pose.position.y,
                                uav_t265.pose.position.z);
    Eigen::Vector3d p_obs_t265 = q_2652vicon * (p_obs_vicon - p_uav_vicon) + p_uav_t265;

    obs_t265.pose.position.x = p_obs_t265(0);
    obs_t265.pose.position.y = p_obs_t265(1);
    obs_t265.pose.position.z = p_obs_t265(2);
    obs_t265.pose.orientation = obs_vicon.pose.orientation;

    pubPose.publish(obs_t265);
    rate.sleep();
  }

  return 0;
}
